使用者插补范例

此范例程式说明如何使用 KINGSTAR 子系统的直接控制模式，直接控制模式能让应用程式透过子系统使用自己的插补及传送循环命令到装置上，子系统接受位置、速度或扭矩直接命令，但非所有驱动器支援扭矩命令。此范例使用位置命令，速度与扭矩命令皆为相同的运作方式。

此范例亦介绍了处理 EtherCAT 通讯、配置 EtherCAT 从站和 KINGSTAR 子系统等的 KINGSTAR 现场总线函式，也介绍了配置轴和设定轴位置的 KINGSTAR Motion 函式，但不包含移动函式，若欲使用移动函式，需使用其中一个主插补模式。

编译及执行范例程式

档案位在：C:\Users\Public\Public Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\UserInterpolationSample，将 UserInterpolationSample.sln 开启并编译之。

注意：档案总管 (File Explorer) 有两个路径：阶层路径与完整路径，阶层路径显示在地址栏中；完整路径显示在档案总管上方。右键点击 UserInterpolationSample.sln 后点选 Properties，将可看见位置 (Location) 为 C:\Users\Public\Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\UserInterpolationSample，此即为完整路径；而注意阶层路径为 Public Documents。若使用非英文的 Windows 系统，而需要复制贴上路径至地址栏中以加快查找范例速度，则请务必使用完整路径；若想要透过点击浏览范例档案夹，则请使用阶层路径。英文版 Windows 之档案夹将自动重新导向，因此就算贴上阶层路径，档案总管亦可引导至范例程式。

下图为范例程式的输出：

在 Visual Studio 中设定专案 properties

范例程式使用 Visual Studio 2019 的 C 语言开发，开发应用程式时，只要此应用程式为 64 位元，即可自行选择开发环境，因控制即时子系统需使用 64 位元。

创建应用程式时需在 Visual Studio 内修改以下属性：

在 Solution Explorer 中的专案名称上点击右键，接著点选 Properties。
进入 (Project name) Property Pages 对话框中的左窗格，点开 C/C++ 清单并点选 General。
在右方区域的 Additional Include Directories 方框输入 "$(RTX64SDKDir4)include;$(KINGSTARSDKDir4)include;%(AdditionalIncludeDirectories)" （无空白鉴）。
在左窗格中点开 Linker 清单后点选 General。
右侧 Additional Library Directories 方框中，输入"$(RTX64SDKDir4)lib\$(Rtx64Platform);$(KINGSTARSDKDir4)lib\amd64;%(AdditionalLibraryDirectories)"（无空白键）。
在左窗格中的 Linker 清单中，点选 Input。
在右侧 Additional Dependencies 方框中，依照建置目标输入以下字串：

RtssDebug 与 RtssRelease 建置：输入不含空白键的 "startupCRT.lib;libcmtd.lib;libcpmtd.lib;libucrtd.lib;libvcruntimed.lib;rtx_rtss.lib;ksapi_rtss.lib"。
Debug 与 Release 建置：输入不含空白键的 "rtapi.lib;ksapi.lib" 。

点击 Apply 后再按一下 OK。
在专案的标头档（名称为 ProjectName.h）之 #endif // UNDER_RTSS 底下，输入以下代码：

#include <ksapi.h>
#include <ksmotion.h>

分组函式

为增加编码的可读性，函式在范例程式中使用 #pragma 分组，透过此 #pragma 码可知函式的使用方式。

执行循环任务

我们定义函式 CyclicTask，此函式在每个周期呼叫。

int CyclicTask(PVOID Context) {
   int TargetAxis = 0;

   // Moving at a fixed velocity of 360 degrees per second
   double Position = 0;
   GetAxisPosition(TargetAxis, mcSetValue, &Position);
   BOOL HomeAbsSwitch;
   BOOL LimitSwitchPos;
   BOOL LimitSwitchNeg;
   BOOL Simulation;
   BOOL CommunicationReady;
   BOOL ReadyForPowerOn;
   BOOL PowerOn;
   BOOL IsHomed;
   BOOL AxisWarning;
   GetAxisInfo(TargetAxis, &HomeAbsSwitch, &LimitSwitchPos, &LimitSwitchNeg, &Simulation, &CommunicationReady, &ReadyForPowerOn, &PowerOn, &IsHomed, &AxisWarning);
   if (PowerOn == TRUE) {
      Position += (double)360 / (double)1000; 
      SetAxisPosition(TargetAxis, Position);
   }
   return 0;
}

两个宣告的变数为：TargetAxis 与 Position。

TargetAxis 为轴索引。
Position 为轴位置。

使用 GetAxisPosition 以取得轴的目前位置，接著使用 GetAxisInfo 来确认轴已被启动，轴启动后可使用 SetAxisPosition 设定轴的目标位置。

配置 KINGSTAR 子系统

开启 KINGSTAR 子系统前，必须先初始化处理序及变数，若发生非预期错误，该呼叫将回传 KsError 类型中的错误码，在子系统成功初始化之前其他函式皆无法使用，所有先前的设定都将被覆写。

在初始化处理序与变数前，我们宣告以下变数，这些变数在 int _tmain 中使用。

SlaveStatus axis = { 0 };
int axisResolution = 10000;
McPidSettings myPid = { 1, 0, 0, 0, 1, 0.003, 0.003, 0, 0.2, 0.1, FALSE, FALSE, 0, 5000 };
McProfileSettings Motion = { 3, 3600, 3600, 36000, 36000, 3600000, 0 };
KsCommandStatus Command = { 0 };
KsError Code = errNoError;

SlaveStatus axis = { 0 } – SlaveStatus 为包含 EtherCAT 从站细节的结构，我们使用 axis 以获取从站资讯。
int axisResolution = 10000 – axisResolution 为轴（伺服驱动器）的解析度。初始值设为 10000，实际值依使用的轴而定。
McPidSettings myPid – McPidSettings 为包含 KINGSTAR Motion PID 设定的结构，因此我们使用 myPid 来初始化 PID 值。
McProfileSettings Motion – McProfileSettings 为包含与运动相关设定的结构，如加速度、减速度与加加速度，因此我们使用 Motion 来初始化运动值。
KsCommandStatus Command = { 0 } – KsCommandStatus 为包含函式状态的结构，可得知函式是否正在执行、完成、回传任何错误及何错误，我们使用 Command 以获取函式状态。
KsError Code = errNoError – KsError 为包含 KINGSTAR 所有错误码的列举类型，errNoError 表示函式已成功。

初始化程序包含在 #pragma region Initialization 内，使用 Code 以检查函式是否成功执行，其将接收函式的回传值，若 Code 非 errNoError，程式将会跳至 End 方块并执行里面的程式码。

准备连接至 KINGSTAR 子系统

首先呼叫 Create，准备连接应用程式至 KINGSTAR 子系统，开始任何动作前，必须先呼叫 Create。

Code = Create(0, 0);
if (Code != errNoError) {
   RtPrintf("Failed to create: 0x%x\n", Code);
   goto End;
}

设定 EtherCAT 循环时间

SetCycleTime设定 EtherCAT 循环时间（秒），其时间单位为秒。在此范例中，cycle1000 代表 1 毫秒，若欲输入一数字，请输入 SetCycleTime(0.001)，我们使用变数以代表数字，可在 ksapi.h 中找到以下的定义，其位于 C:\Program Files\IntervalZero\KINGSTAR SDK\4.0\Include。

变数	数字
cycle100	0.0001
cycle125	0.000125
cycle250	0.000250
cycle500	0.0005
cycle1000	0.001

欲使用低于 1 毫秒的循环时间，须备有高速计时器套件。注意非所有轴皆支援快速循环时间，若选择了不支援的循环时间，则每个轴的更新时间会自动且各自延长，欲使用快速循环时间，请确保电脑上的网卡可使用，只有具有低延迟的网卡才可支持快速循环。

Code = SetCycleTime(cycle1000);
if (Code != errNoError) {
   RtPrintf("Failed to set cycle time: 0x%x\n", Code);
   Destroy();
   goto End;
}

停用 RTX64 伺服主控台 (Server Console) 上的记录

EnableServerLog启用或停用 RTX64 Server Console 上的即时讯息，若将其停用，则主控台将只显示 KINGSTAR 讯息，因此我们选择将其停用。

Code = EnableServerLog(FALSE);
if (Code != errNoError) {
   RtPrintf("Failed to set server log: 0x%x\n", Code);
   Destroy();
   goto End;
}

设定存取模式

SetAxisAccessMode设定 EtherCAT 驱动器之资料传送模式，存取模式决定轴可用的控制模式，存取模式可在 KsAccessMode 列举类型中选择，预设之存取模式为 accessVelPos，而我们使用 accessPosVel。

Code = SetAxisAccessMode(accessPosVel);
if (Code != errNoError) {
   RtPrintf("Failed to set access mode: 0x%x\n", Code);
   Destroy();
   goto End;
}

启用轴之数位输入

EnableAxisInput启用或停用轴之数位输入的存取，首三个输入位元为负超程 (Overtravel)、正超程与原点复归感测器，若输入启用后可使用超程位元。

Code = EnableAxisInput(TRUE);
if (Code != errNoError) {
   RtPrintf("Failed to set servo inputs: 0x%x\n", Code);
   Destroy();
   goto End;
}

启用热插拔 (Hot Connect) 并设定虚拟轴的数量

EnableHotConnect允许在EtherCAT 网路运行时添加新的硬体。SetConfiguredAxesCount 设定模拟轴的数量，在此范例中使用一个模拟轴。

Code = EnableHotConnect(TRUE);
Code = SetConfiguredAxesCount(1);

完成子系统配置

KINGSTAR 提供许多配置子系统之设定，此范例仅使用其中一部分，其他设定可至线上或离线帮助查看 > KINGSTAR RT 与 Win32 API > KINGSTAR 总线 > Functions > 轴变数。

更多设定为选择性的，但有两个设定为必设：EtherCAT 循环时间和存取模式，若两者设定的预设值皆不符所需，请将值更改为更适合的值。

在此范例中使用以下代码以表示子系统配置已完成：

RtPrintf("Subsystem configured\n");

输出：

开启 KINGSTAR 子系统

我们使用 Start 来开启 KINGSTAR 子系统和 EtherCAT 网路，当子系统无法启动造成程序永远在等待完成，我们为 Start 使用 WaitForCommand 将超时设置为 30 秒；若 KINGSTAR 子系统无法启动，我们使用 Destroy 关闭与子系统的连接并将其终止。

Command = WaitForCommand(30, TRUE, Start());
if (!Command.Done) {
   RtPrintf("Failed to start EtherCAT: 0x%x\n", Command.ErrorId);
   Destroy();
   goto End;
}
RtPrintf("Subsystem Started\n");

输出：

子系统开启后，欲撷取其当前状态，使用 SubsystemStatus 结构及 GetStatus 函式。

SubsystemStatus Subsystem = { ecatOffline, ecatOffline, 
0, 0, 0, {ecatOffline}, {ecatOffline}, {axisOffline} };

SubsystemStatus 包含 EtherCAT 网路的详细资讯，可从其得知主站和从站的 EtherCAT 状态、所有 EtherCAT 从站的数量、EtherCAT 网路上的轴与 I/O 模组及其他。我们使用变数 Subsystem 以获取 EtherCAT 网路的资讯，并分派初始值至 Subsystem，以分辨当资料传送过去时 Subsystem 的内容是否改变。

Code = GetStatus(&Subsystem, NULL);
if (Code != errNoError) {
   RtPrintf("Failed to get status: 0x%x\n", Code);
   goto Exit;
}
RtPrintf("Subsystem status\n  %d Slaves\n  %d I/Os\n  %d Axes\n\n", 
	Subsystem.SlaveCount, Subsystem.IOCount, Subsystem.AxesCount);

GetStatus获得所创建的 EtherCAT 网路状态，此状态传送至 Subsystem，因为不使用 NULL，我们将其填入第二参数，接著再使用 Code 以检查 GetStatus 是否成功执行，若成功，则包含 EtherCAT 从站数量、轴、I/O 模组的子系统状态将会显示于 RTX64 伺服器主控台。

输出：

轴配置

移动轴前须先配置其设定，在配置任何设定前我们宣告以下变数：

int TargetAxis = 0;

TargetAxis 为轴索引。此变数在 #pragma region AxisConfiguration 区块中使用。

获取轴状态

使用 GetAxisByIndex 以获取轴状态，将 TargetAxis 设定为零，代表我们欲获取索引为零的轴资讯。

接著呼叫 GetAxisByIndex，axis 为 SlaveStatus 变数，axisResolution 为我们在 int _tmain 函式开头所宣告的变数，初始值为 10000，因为不使用最后两个参数，我们将之设为 NULL。

Code = GetAxisByIndex(TargetAxis, &axis, &axisResolution, NULL, NULL);
if (Code != errNoError) {
   RtPrintf("Failed to get axis status: 0x%x\n", Code);
   goto Exit;
}
RtPrintf("Axis status\n  Name: %s\n  Vendor: 0x%x\n  Product: 0x%x\n  
	Revision: %d\n  Alias: %d\n  Explicit ID: %d\n\n", axis.Name, 
	axis.VendorId, axis.ProductCode, axis.RevisionNumber, 
	axis.AliasAddress, axis.ExplicitId);

我们使用 Code 以检查函式是否如往常一样成功执行，若成功，轴零号资讯将会显示在 RTX64 主控台。

输出：

转换轴单位

使用 SetAxisCountsPerUnit 以将轴位置单位转换为度数，如此可易于知道轴移动了多远，可使用此函式以设定想要的单位。

Code = SetAxisCountsPerUnit(TargetAxis, axisResolution, 360, FALSE);
if (Code != errNoError) {
   RtPrintf("Failed to set axis unit: 0x%x\n", Code);
   goto Exit;
}

启用单位转换

欲套用在 SetAxisCountsPerUnit 中设定的单位，需使用 EnableAxisUnitConversion 以启用转换。

Code = EnableAxisUnitConversion(TargetAxis, TRUE);
if (Code != errNoError) {
   RtPrintf("Failed to enable axis unit: 0x%x\n", Code);
   goto Exit;
}

设定轴的控制模式

我们使用 SetAxisControlMode 以设定轴的控制模式，因此范例使用直接位置 (Direct Position) 模式，所以我们将控制模式设为 modeDirectPos，为避免函式执行过长，我们使用 WaitForCommand 以为 SetAxisControlMode 设定 1 秒超时。

Command = WaitForCommand(1, TRUE, SetAxisControlMode(TargetAxis, modeDirectPos));
if (!Command.Done) {
   RtPrintf("Failed to set axis control mode: 0x%x\n", Command.ErrorId);
   goto Exit;
}

更新 PID 与运动参数

单位转换后必须更新 PID 及运动参数，我们使用 SetAxisVelocityPid 来更新速度模式中的 PID 设定（若轴在扭矩模式则可使用 SetAxisTorquePid）；而 SetAxisMotionProfile 配置轴的运动设定，在 int _tmain 开头所宣告的实例 Motion 在此套用。

Code = SetAxisVelocityPid(TargetAxis, myPid);
if (Code != errNoError) {
   RtPrintf("Failed to set axis PID: 0x%x\n", Code);
   goto Exit;
}
Code = SetAxisMotionProfile(TargetAxis, profileUnitPerSecond, Motion);
if (Code != errNoError) {
   RtPrintf("Failed to set axis motion profile: 0x%x\n", Code);
   goto Exit;
}

完成轴配置

KINGSTAR 提供多个配置轴的设定，此范例只使用了其中几个，其他设定资讯可至线上或离线帮助查看 > KINGSTAR RT 与 Win32 API > KINGSTAR Motion > Functions > 轴配置。

大多设定为选择性的，但控制模式为必设。

在此范例中使用以下代码以表示轴已配置完成：

RtPrintf("Axis configured\n");

输出：

每周期检查轴的操作状态

我们使用 RegisterCallback 已注册 CyclicTask 因而让 KINGSTAR 子系统在每周期有新资料进来时呼叫之，CyclicTask 以恒定速度移动轴。更多关于 CyclicTask 之资讯请见执行循环任务。

Code = RegisterCallback(&CyclicTask, NULL);

启动轴

轴完成配置后即可启动与移动轴。

重设警报

启动轴之前，我们使用 ResetAxis 以重设轴的警报，以防有任何错误，为避免函式执行过长，我们使用 WaitForCommand 以为 ResetAxis 设定 5 秒超时。

Command = WaitForCommand(5, TRUE, ResetAxis(TargetAxis));
if (!Command.Done) {
   RtPrintf("Failed to reset the axis: 0x%x\n", Command.ErrorId);
   goto Exit;
}

启动轴

使用 PowerAxis 以启动轴，此函式中的第一个参数为轴索引、第二个参数控制轴的电源，而第三及第四个参数控制轴移动的方向，为避免函式执行过长，我们使用 WaitForCommand 以为 PowerAxis 设定 1 秒超时，

Command = WaitForCommand(1, FALSE, PowerAxis(TargetAxis, TRUE, TRUE, TRUE));
if (!Command.Done) {
   RtPrintf("Failed to enable the axis: 0x%x\n", Command.ErrorId);
   goto Exit;
}
RtPrintf("Axis enabled\n");

轴启动以后让轴移动一阵子。

Sleep(30000);

停用轴

轴移动一阵子后，使用 PowerAxis 以停用轴。

Command = WaitForCommand(1, FALSE, PowerAxis(TargetAxis, FALSE, TRUE, TRUE));
if (!Command.Done) {
   RtPrintf("Failed to disable the axis: 0x%x\n", Command.ErrorId);
   goto Exit;
}
RtPrintf("Axis disabled\n");

输出：

停止 KINGSTAR 子系统

我们使用两个函式来停止 KINGSTAR 子系统：Stop 和 Destroy。

Stop停止 EtherCAT 网路与 KINGSTAR 子系统。
Destroy关闭至 KINGSTAR 子系统的连结并终止该子系统。

因子系统停止需要一些时间，我们使用 WaitForCommand 以提供 10 秒的停止时间，若子系统在 10 秒到后未停止，下一个指令 Destroy 将会执行。

Exit:
//Stop EtherCAT
Command = WaitForCommand(10, FALSE, Stop());
if (!Command.Done) {
   RtPrintf("Failed to stop EtherCAT: %d\n", Command.ErrorId);
}
else {
   RtPrintf("EtherCAT stopped\n");
}
//Stop the Subsystem.
//Never stop the Subsystem if another application or the PLC is running. It would cause a BSOD.
Code = Destroy();
if (Code != errNoError) {
   RtPrintf("Destroy failed: %d\n", Code);
}

Stop和 Destroy 皆包含在 Exit 区块内，其在子系统未成功启动时执行，使用这两个函式时不需要将它们包含在任何区块内，可以依照需要使用。

End 区块在函式未成功运行时执行。

End:
RtPrintf("Basic Sample ended\n");

输出：